import threading
import time
from math import pow, sqrt
import datetime

from dji_asdk_to_python.products.aircraft import Aircraft
from dji_asdk_to_python.flight_controller.flight_controller_state import (
    FlightControllerState,
)

# Setting global variables

APP_IP = "192.168.20.37"
FREQ_SEND = 5  # hz

REQUEST_DELAY = 3  # sec
MIN_ELAPSED_TIME = 300  # seconds
IS_FLYING = False

# Initializing aircraft object to handle drone

aircraft = Aircraft(APP_IP)
print(" ############################## Starting Telemetry process in DJI")

time_0 = time.time()
flight_controller = aircraft.getFlightController()
start = time_0
elapsed = 0
flying = IS_FLYING
count = 0

# Loop to do actions while time be lesser than min allowed or drone is flying}
#i = 0
print("############################## Starting Telemetry loop before flying")
import datetime
while elapsed < MIN_ELAPSED_TIME or flying:
#while i != 2:
    #i += 1
    # Updating time variables
    if time.time() >= time_0 + 1.0 / FREQ_SEND:
        now = datetime.datetime.now()
        print ("Current date and time : ")
        print (now.strftime("%Y-%m-%d %H:%M:%S"))
        count+=1
        print(count)
        time_0 = time.time()
        elapsed = time_0 - start

        # Getting battery state and flight controller state

        battery = aircraft.getBattery()
        battery_state = battery.getBatteryState()
        flight_controller_state = flight_controller.getState()

        # Evaluating a correct flight controller state

        if not isinstance(flight_controller_state, FlightControllerState):
            continue
        
        # Getting parameters for drone in flight

        flying = flight_controller_state.isFlying()
        aircraft_attitude = flight_controller_state.getAttitude()
        aircraft_location = flight_controller_state.getAircraftLocation()

        remaining_battery = battery_state.getChargeRemainingInPercent()

        aircraft_latitude = aircraft_location.getLatitude()
        aircraft_longitude = aircraft_location.getLongitude()

        # Initializing and sending telemetry data message and setting its parameters
        print("LATITUDE: ",str(aircraft_latitude))
        print("LONGITUDE: ",str(aircraft_longitude))

print(" ############################## Closing Telemetry Updater")
